#include "kf_lateral.h"
#include <iostream>
#include <vector>
#include <random>
#include "matplotlibcpp.hpp"
#include <cmath>
#include "test_kf_lateral_path.h"
#include <yaml-cpp/yaml.h>
#include "common_tool/csv_parser.h"
namespace plt = matplotlibcpp;
using namespace KalmanFilter;
using namespace auto_ros::control;
int main()
{
	//KfLateral(double mass, double cf, double cr, double lf, double lr, double iz, double rate,
	//Eigen::MatrixXd &sys_covariance, Eigen::MatrixXd &measure_covariance);
	std::string dyn_config_filename = test_kf_lateral_path + "dyn_lateral.yaml";
	std::string kf_config_filename = test_kf_lateral_path + "kf_lateral.yaml";
	std::string csv_fileanme = test_kf_lateral_path + "u_15_96k_2rad.csv";
	YAML::Node dyn_yaml = YAML::LoadFile(dyn_config_filename);
	YAML::Node kf_yaml = YAML::LoadFile(kf_config_filename);
	Eigen::MatrixXd sys_covariance(6, 6);
	Eigen::MatrixXd measure_covariance(4, 4);
	sys_covariance << std::pow(kf_yaml["std_beta"].as<double>(), 2), 0, 0, 0, 0, 0,
		0, std::pow(kf_yaml["std_yaw_rate"].as<double>(), 2), 0, 0, 0, 0,
		0, 0, std::pow(kf_yaml["std_e_y"].as<double>(), 2), 0, 0, 0,
		0, 0, 0, std::pow(kf_yaml["std_e_gama"].as<double>(), 2), 0, 0,
		0, 0, 0, 0, std::pow(kf_yaml["std_beta_dis"].as<double>(), 2), 0,
		0, 0, 0, 0, 0, std::pow(kf_yaml["std_yaw_rate_dis"].as<double>(), 2);

	measure_covariance << std::pow(kf_yaml["std_beta_m"].as<double>(), 2), 0, 0, 0,
		0, std::pow(kf_yaml["std_yaw_rate_m"].as<double>(), 2), 0, 0,
		0, 0, std::pow(kf_yaml["std_e_y_m"].as<double>(), 2), 0,
		0, 0, 0, std::pow(kf_yaml["std_e_gama_m"].as<double>(), 2);
	double mass = dyn_yaml["mass"].as<double>();
	double m = mass;
	double cf = dyn_yaml["cf"].as<double>();
	double cr = dyn_yaml["cr"].as<double>();
	double lf = dyn_yaml["lf"].as<double>();
	double lr = dyn_yaml["lr"].as<double>();
	double iz = dyn_yaml["iz"].as<double>();
	KfLateral kf_lateral(dyn_yaml["mass"].as<double>(), dyn_yaml["cf"].as<double>(), dyn_yaml["cr"].as<double>(),
						 dyn_yaml["lf"].as<double>(), dyn_yaml["lr"].as<double>(), dyn_yaml["iz"].as<double>(), 50,
						 sys_covariance, measure_covariance);

	//KfLateral::reset(Eigen::VectorXd state_esti, Eigen::MatrixXd state_cov_esti)
	Eigen::VectorXd state_esti(6);
	state_esti << 0, 0, 0, 0, 0, 0;
	Eigen::MatrixXd state_cov_esti(6, 6);
	state_cov_esti << 0.1, 0, 0, 0, 0, 0,
		0, 0.1, 0, 0, 0, 0,
		0, 0, 0.1, 0, 0, 0,
		0, 0, 0, 0.1, 0, 0,
		0, 0, 0, 0, 0.1, 0,
		0, 0, 0, 0, 0, 0.1;

	kf_lateral.reset(state_esti, state_cov_esti);
	io::CSVReader<7> in(csv_fileanme);
	in.read_header(io::ignore_extra_column, "beta", "yaw_rate", "e_y", "e_heading", "kappa", "tire_f_angle", "speed");
	double beta = 0;
	double yaw_rate = 0;
	double e_y = 0;
	double e_heading = 0;

	double kappa = 0;
	double speed = 0;
	double tire_f_angle = 0;
	double last_f_angle = 0;

	std::vector<double> beta_vector;
	std::vector<double> beta_esti_vector;
	std::vector<double> yaw_rate_vector;
	std::vector<double> yaw_rate_esti_vector;
	std::vector<double> e_y_vector;
	std::vector<double> e_y_esti_vector;
	std::vector<double> e_heading_vector;
	std::vector<double> e_heading_esti_vector;

	std::vector<double> delta_beta_vector;
	std::vector<double> delta_yaw_rate_vector;
	std::vector<double> beta_dis_vector;
	std::vector<double> yaw_rate_dis_vector;
	std::vector<double> ref_beta_vector;
	std::vector<double> ref_beta_vector1;
	std::vector<double> ref_f_angle_vector;
	std::vector<double> ref_f_angle_vector1;
	std::vector<double> f_angle_vector;
	while (in.read_row(beta, yaw_rate, e_y, e_heading, kappa, tire_f_angle, speed))
	{
		beta_vector.push_back(beta);
		yaw_rate_vector.push_back(yaw_rate);
		e_y_vector.push_back(e_y);
		e_heading_vector.push_back(e_heading);
		//bool step(Eigen::VectorXd &control_last, Eigen::VectorXd &y, double speed);
		Eigen::VectorXd control_last(2);
		control_last << last_f_angle, kappa;
		Eigen::VectorXd y(4);
		y << beta, yaw_rate, e_y, e_heading;
		kf_lateral.step(control_last, y, speed);
		Eigen::VectorXd state_esti = kf_lateral.state_esti();
		beta_esti_vector.push_back(state_esti(0));
		yaw_rate_esti_vector.push_back(state_esti(1));
		e_y_esti_vector.push_back(state_esti(2));
		e_heading_esti_vector.push_back(state_esti(3));
		beta_dis_vector.push_back(state_esti(4));
		yaw_rate_dis_vector.push_back(state_esti(5));
		double distur_beta = state_esti(4);
		double distur_ay = distur_beta;
		double distur_kappa = kappa;
		double distur_yaw = state_esti(5);
		double distur_yaw_aacc = distur_yaw;
		double ref_beta = distur_beta * lr / speed +
						  distur_kappa * (cr * lf * lr + cr * lr * lr - lf * mass * speed * speed) / (cr * (lf + lr)) -
						  distur_yaw * iz / (cr * (lf + lr));
		double ref_f_angle = 1.0 * (1.0 * cf * cr * distur_kappa * lf * lf + 2.0 * cf * cr * distur_kappa * lf * lr + 1.0 * cf * cr * distur_kappa * lr * lr + 1.0 * cf * distur_ay * lf * m - 1.0 * cf * distur_kappa * lf * m * speed * speed - 1.0 * cf * distur_yaw_aacc * iz - 1.0 * cr * distur_ay * lr * m + 1.0 * cr * distur_kappa * lr * m * speed * speed - 1.0 * cr * distur_yaw_aacc * iz) / (cf * cr * (lf + lr));
		double ref_f_angle1 = distur_kappa * ((cf + cr) * (cf * lf * lf + cr * lr * lr) - (cf * lf - cr * lr) * (cf * lf - cr * lr + mass * speed * speed)) / (cf * (-cf * lf + cr * lr + lf * (cf + cr)));
		ref_f_angle_vector.push_back(ref_f_angle);
		ref_f_angle_vector1.push_back(ref_f_angle1);
		ref_beta_vector.push_back(ref_beta);
		ref_beta_vector1.push_back(distur_kappa * (cr * lf * lr + cr * lr * lr - lf * mass * speed * speed) / (cr * (lf + lr)));
		last_f_angle = tire_f_angle;
		f_angle_vector.push_back(tire_f_angle);
	}
	plt::plot(beta_vector);
	plt::plot(beta_esti_vector);
	plt::plot(beta_dis_vector);
	plt::show();

	plt::plot(yaw_rate_vector);
	plt::plot(yaw_rate_esti_vector);
	plt::plot(yaw_rate_dis_vector);
	plt::show();

	/*plt::plot(e_y_vector);
	plt::plot(e_y_esti_vector);
	plt::show();

	plt::plot(e_heading_vector);
	plt::plot(e_heading_esti_vector);
	plt::show();

	plt::plot(ref_beta_vector);
	plt::plot(ref_beta_vector1);
	plt::show();
	plt::plot(ref_f_angle_vector1);
	plt::plot(ref_f_angle_vector);
	plt::plot(f_angle_vector);
	plt::show();*/
	plt::plot(ref_f_angle_vector);
	plt::plot(ref_f_angle_vector1);
	plt::show();
}